#ifndef PCL_NO_PRECOMPILE
#define PCL_NO_PRECOMPILE
#endif

#include <memory>
#include <chrono>
#include <string>
#include <boost/make_shared.hpp>
#include <vector>
#include <cmath>
#include "rclcpp/rclcpp.hpp"
//#include "std_msgs/msg/string.hpp"
#include <sensor_msgs/msg/laser_scan.hpp>
#include "sensor_msgs/msg/point_cloud2.hpp"

#include "sensor_msgs/msg/imu.hpp"
#include "std_msgs/msg/header.hpp"

#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
//#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
//#include <pcl/octree/octree.h>
#include <pcl/kdtree/kdtree_flann.h>
//#include <pcl/registration/icp.h>
//#include <pcl/features/normal_3d.h>

// Eigen
#include <Eigen/Core>
#include <Eigen/Eigen>
#include <Eigen/Geometry>

#include <fstream>
#include "scan_process.hpp"
#include "fuction_temp.hpp"
#include "seat_conver.hpp"
using namespace std;
#define SCAN_CLOUD 0
class BlueseaSubScan: public rclcpp::Node
{
    public:
        BlueseaSubScan(): Node("process_bluesea")
        {
            using std::placeholders::_1;
            scan_p=new Scan_cloud();
           // init_cloud = new Scan_cloud();
            if(SCAN_CLOUD==0)
            sub_bluesea_cloud = this->create_subscription<sensor_msgs::msg::PointCloud2>(
                "/cloud", 
                10, std::bind(&BlueseaSubScan::topic_callback_cloud, this, std::placeholders::_1));
           if(SCAN_CLOUD==1)
            sub_bluesea_scan = this->create_subscription<sensor_msgs::msg::LaserScan>(
                "/scan", 
                10, std::bind(&BlueseaSubScan::topic_callback_scan, this, std::placeholders::_1));
            publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/my_cloud", 10);
            fseat.load_seat_pcd("seat0.pcd");
            publisher_init_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/my_init_cloud", 10);
              fseat.set_seat_zero();
        }
    private:
      void topic_callback_scan(const sensor_msgs::msg::LaserScan::SharedPtr scan) 
     {
         sensor_msgs::msg::PointCloud2 mymsg;
         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ (new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ> angle_cloud;
        scan_p->set_scan_input(scan);
        scan_p->convert_to_cloud(cloud_);
        scan_p->find_angle_point();
        std::size_t size=scan_p->angle_index.size();
        RCLCPP_INFO(this->get_logger(),"angle sum=%d ",size);
        for(int i=0;i<size;i++)
          angle_cloud.push_back(cloud_->points[scan_p->angle_index[i].index]); 
        //pcl::toROSMsg(* cloud_, mymsg);
        pcl::toROSMsg(angle_cloud, mymsg);
        mymsg.header.stamp =now();
        mymsg.header.frame_id="map";
        publisher_->publish(mymsg);
     }
     void topic_callback_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg) 
     {
         size_t size;
         double x,y,max_x,max_y;
         RCLCPP_INFO(this->get_logger(),"process start "); 
         pcl::PointCloud<pcl::PointXYZ> lidar_cloud;
        
         sensor_msgs::msg::PointCloud2 mymsg;
         pcl::fromROSMsg(*msg, lidar_cloud);
            size =lidar_cloud.points.size();
       // for(int32_t i=0;i<20;i++)    
       //   RCLCPP_INFO(this->get_logger(),"i=%d  x=%5.2f y=%5.2f,z=%5.2f",i,lidar_cloud.points[i].x,lidar_cloud.points[i].y,lidar_cloud.points[i].z);
        pcl::PointCloud<pcl::PointXYZ>::Ptr trybb_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ> temp_cloud,temp2_cloud;
        move_stable_point(lidar_cloud,temp_cloud);
        move_single_point(temp_cloud,*trybb_cloud);
        temp_cloud.points.clear();

        pcl::PointXYZ seat;
        seat.x=0.8;
        seat.y=-0.8;
        seat.z=0.0;
        seat_transform(seat,0,*trybb_cloud,temp2_cloud);   
      
        fseat.set_input_coud(temp2_cloud);
        // trysave++;
        // if(trysave==10)
        // {
        //   pcl::io::savePCDFile("seat0.pcd",temp2_cloud);
        //    RCLCPP_INFO(this->get_logger(),"save file ");   
        // }

        // temp_cloud.height=temp_cloud.points.size();
        // temp_cloud.width=1;
            pcl::toROSMsg(temp2_cloud, mymsg);
            // mymsg.height=temp_cloud.points.size();
            // mymsg.width=1;
            mymsg.header.stamp = now();
            mymsg.header.frame_id="map";
           //  publisher_->publish(*msg);
            publisher_->publish(mymsg);

             sensor_msgs::msg::PointCloud2 init_msg;
             size=fseat.init_seat_cloud.points.size();
              pcl::toROSMsg(fseat.init_seat_cloud, init_msg);
               init_msg.header.stamp = now();
            init_msg.header.frame_id="map";
            publisher_init_->publish(init_msg);
          RCLCPP_INFO(this->get_logger(),"process end %d",size);   
          
     }
    
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_bluesea_cloud;
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr sub_bluesea_scan;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_init_;
    Scan_cloud *scan_p;
  //  Scan_cloud *init_cloud;
    int trysave=0;


    find_seat fseat;
};


int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<BlueseaSubScan>());
  
  rclcpp::shutdown();
  return 0;
}


